#include "ros/ros.h"
#include "nxt_lab2/MoveToCell.h"
#include "nxt_lab2/Neighbors.h"
#include <ros/ros.h>
#include <iostream>
#include <stdio.h>

#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/TwistWithCovariance.h>

#include <std_msgs/Float64.h>
#include <nxt_msgs/Range.h>
#include <nxt_msgs/Gyro.h>
#include <nxt_msgs/Color.h>

#include <sensor_msgs/JointState.h>

#include <tf/transform_broadcaster.h>

#include <nav_msgs/Odometry.h>

using namespace std;

/************** PARAMETRI DEFINITI SULLA BASE DEL ROBOT E DELLA GRIGLIA UTILIZZATA *******************/

double pi = 3.14159265358979323846; // la costante geometrica pi-greco
double publication_frequency = 10;  // frequenza a cui i sensori pubblicano le info, in Hz
double rot_speed = 1.55;            // velocità di rotazione del robot
double linear_speed = 0.11;         // velocità di avanzamento rettilineo
double range_to_obstacle = 0.45;     // range a cui il sonar interpreta una cella occupata
double wheel_radius = 0.022;   // circonferenza di una ruota del robot
double cell_width = 0.39;           // è la larghezza di una cella della griglia

/************** VARIABILI CALIBRATE A RUNTIME  **************************************/

double cell_intensity = 0;          // è l'intensità a cui viene supposto il verde della cella
double gyro_angular_z_error = 0;    // errore sistematico del giroscopio, viene aggiornato in fase di calibrazione

/************** VARIABILI COSTANTEMENTE AGGIORNATE DAI CALLBACK *************************************/

double gyro_angular_z = 0;          // è costantemente aggiornata alla velocità angolare attuale del giroscopio
double gyro_integration = 0;        // accumula nel tempo le misurazioni del giroscopio (quindi va diviso per la pubblication_frequency!)
double range = 2.5;                 // costantemente aggiornato al range attuale del sonar
double intensity = 1023;            // costantemente aggiornato all'intensità del sensore di luminosità

vector<double> joint_position;      // vettori costantemente aggiornati con lo stato attuale dei giunti
vector<double> joint_effort;
vector<double> joint_velocity;

/************** VARIABILI GLOBALI DI STATO UTILIZZATE NEL CODICE ***********************************/

int orientation = 0;                // INIZIA SEMPRE ORIENTATO A NORD

ros::Publisher velocity_pub, head_angle_pub;


/***************** CALLBACKS PER L'AGGIORNAMENTO DELLE VARIABILI ***********************************/

/*
  Rende disponibili le informazioni sul giroscopio tramite
  la variabile globale gyro_angular_z.
  
  Si occupa anche di integrare nel tempo
  le informazioni sulle velocità angolari.

  Azzerando la variabile globale gyro_integration il conteggio
  dell'angolo percorso riparte da 0.
*/
void callbackGyro(const nxt_msgs::Gyro::ConstPtr& msg) {

  //cout << "Gyro angular velocity: " << msg->angular_velocity.z-gyro_angular_z_error << endl;
  gyro_angular_z = msg->angular_velocity.z - gyro_angular_z_error;
  gyro_integration += gyro_angular_z/publication_frequency;
  //cout << "gro " << gyro_angular_z/publication_frequency << " " << gyro_integration << endl;
}

void callbackUltrasonicSensor(const nxt_msgs::Range::ConstPtr& msg)
{
  //cout << "Obstacle at " << msg->range << endl;
  range = msg->range;
}

void callbackIntensitySensor(const nxt_msgs::Color::ConstPtr& msg)
{
  //cout << "Intensity: " << msg->intensity << endl;
  intensity = msg->intensity;
}

void callbackJointStates(const sensor_msgs::JointState::ConstPtr& msg)
{
  joint_position = msg->position;
  joint_effort = msg->effort;
  joint_velocity = msg->velocity;
}

/************************ FUNZIONI DI CALIBRAZIONE ***************************************/

/*
  Calibra il giroscopio facendo una media su un insieme di campioni acquisiti
  a robot fermo.

  samples: è il numero di campioni da utilizzare per la calibrazione.
*/
void calibrateGyro(int samples, ros::Rate r) {
  
  gyro_angular_z_error = 0.0; // inizio la calibrazione da un valore neutro

  double integrated_error = 0;
  double variance = 0;

  for(int i=0; i<samples; i++) { // non uso gyro_integration perchè non avrei controllo sul
                   //    numero di samples effettivamente acquisiti
    
    integrated_error += gyro_angular_z;

    variance += gyro_angular_z*gyro_angular_z;
    //cout << integrated_error << " " << gyro_angular_z << endl;
    r.sleep();
    //ros::spinOnce();
  }

  gyro_angular_z_error = integrated_error/samples;
  //cout << "GIROSCOPE SYSTEMATIC ERROR: " << gyro_angular_z_error << endl;

  variance = variance/samples - integrated_error/samples*integrated_error/samples;
  //cout << "GIROSCOPE SYSTEMATIC ERROR VARIANCE (high is bad): " << variance << endl;
}

/*
  Inizializza l'intensità interna della cella
*/
void calibrateIntensitySensor(ros::Rate r) {
  
  // faccio un po' di cicli di aggiornamento per evitare di prendere valori spuri
  r.sleep();
  ros::spinOnce();
  r.sleep();
  ros::spinOnce();
  r.sleep();
  ros::spinOnce();
  r.sleep();
  ros::spinOnce();
  r.sleep();
  ros::spinOnce();
  r.sleep();
  ros::spinOnce();

  cell_intensity = intensity;
  
  cout << "CELL INTENSITY CALIBRATED AT: " << cell_intensity << endl;
}

/********************* FUNZIONI PER IL MOVIMENTO DEL ROBOT **************************/

/*
  ferma il robot
*/
void stop(ros::Publisher velocity_pub, ros::Rate r) 
{
  geometry_msgs::Twist vel;
  velocity_pub.publish(vel);
  r.sleep();
  //ros::spinOnce();
}

/*
  È la condizione di default, usata dalle funzioni rotate e goStraight
*/
bool stdCondition()
{
  return false;
}

/*
  Ritorna true se il sensore ad ultrasuoni rileva una cella occupata.
*/
bool occupiedCell()
{
   ros::spinOnce();
  return range < range_to_obstacle;
}

/*
  Ritorna true se il robot si trova sopra ad una linea bianca.
*/
bool overTheLine()
{
   ros::spinOnce();
  return intensity > cell_intensity+90;
}

/*
  Ruota il robot di rad radianti basandosi sulle informazioni ottenute
  dal giroscopio per verificare l'angolo effettivamente percorso.
  
  Il controllo sulla velocità angolare del robot non garantisce step abbastanza
  fini di rotazione, quindi la funzione rotate() ritorna un double
  che indica l'angolo effettivamente percorso.

  Parametri
    double rad: indica di quanto il robot deve ruotare su se stesso (positivo --> senso antiorario)
    bool stop_condition: è una funzione che se valutata a true termina la rotazione
*/
double rotate(ros::Publisher velocity_pub, ros::Rate r, double rad, bool (*stop_condition)() = stdCondition) {
  
  //ROS_INFO("ROTATING BODY: %f",rad*360/(2*pi));

  if (abs(rad) < 0.03)
  {
    return 0;
  }

  double correction = 0.05; // radianti

  geometry_msgs::Twist vel;
  vel.angular.z = ( rad >= 0 ) ? rot_speed : -rot_speed; // la veloctià di rotazione viene mantenuta fissa e la direzione dipende dal segno dell'angolo
  rad = ( rad >= 0 ) ? rad+correction : rad-correction;

  gyro_integration = 0.0; // azzero l'integrale del giroscopio per misurare l'angolo percorso nella rotazione corrente
  
  while (abs(gyro_integration) < abs(rad) && !stop_condition())
    {
      //cout << "gyro_integration: " << gyro_integration << " target: " << rad << endl;
    velocity_pub.publish(vel);
      r.sleep();
      //ros::spinOnce();
  }

  stop(velocity_pub, r);

  //cout << "ROTATION ERROR: " << gyro_integration + rad << " rad";
  //cout << " --> " << 360*(gyro_integration + rad)/(2*pi) << " grad" << endl;

  //cout << "EFFECTIVE ROTATION REACHED: " << gyro_integration*360/(2*pi) << "°" << endl;

  return gyro_integration; // lo ritorno perchè è l'effettiva rotazione compiuta,
                           //   può differire di molto dalla rotazione richiesta!!
}

/*
  Questo metodo fa procedere il robot lungo una retta fino al raggiungimento della distanza
  o al raggiungimento della condizione stop_condition.

  Parametri
    double seconds: se > 0 indica per quanti secondi devo avanzare, se invece è == 0 allora avanzo
            fino alla distance_to_cone fissata.
    bool stop_condition: è una funzione che se valutata true termina l'avanzamento del robot.

*/
double goStraight(ros::Publisher velocity_pub, ros::Rate r, double length = 0, bool (*stop_condition)() = stdCondition) {

  //cout << "GOING STRAIGHT..." << endl;

  // TODO: provare con lo spinOnce a vedere se forzando l'aggiornamento va dritto,
  //       altrimenti disabilitare completamente il controllo in retroazione visto che sul pavimento scivolava, invece sul tapeto no!
  // OSS: forzare degli spin del giroscopio non dovrebbe causare problemi perchè vengono tutti integrati, quindi non si perde niente.
  //      inoltre forzare uno spin blocca l'esecuzione ed attende lo spin, cosa che lo spin generale non penso faccia.

  gyro_integration = 0; // azzero l'integrazione del giroscopio
  double systematic_error = 0.0001;

  double position_0 = joint_position[0];
  double distance = 0;

  geometry_msgs::Twist vel;
  vel.linear.x = 0.005; // la velocità aumenta gradualmente a partire da un valore molto basso!

  while ( distance < length && !stop_condition())
    {

      distance = (joint_position[0] - position_0) * wheel_radius;
      
      if(vel.linear.x < linear_speed) // la velocità lineare aumenta gradualmente fino al valore obiettivo massimo
        vel.linear.x += 0.005;

      vel.angular.z = systematic_error;
      //vel.angular.z = gyro_integration/3.0; // setto la  velocità angolare perchè porti a zero l'integrale del giroscopio
      velocity_pub.publish(vel);
      
      r.sleep();
      ros::spinOnce(); // FORZO UNO SPIN PER L'AGGIORNAMENTO DEL GIROSCOPIO!
  }

  stop(velocity_pub, r);

  return distance;
}

/*
  Ruota la testa di un certo angolo. Continua a pubblicare fino a quando la testa non è arrivata all'angolo richiesto.

  Parametri
    double angle: l'angolo in gradi, positivo se senso orario, negativo se antiorario;
*/
void rotateHead(ros::Publisher head_angle_pub, ros::Rate r, double angle) {

  //cout << "ROTATING HEAD: " << angle << " °" << endl;

  std_msgs::Float64 head_angle;
  head_angle.data = angle;

  while(abs(angle/18-joint_position[2]) > 0.4) // angle/18 è preso dal central_joint_controller.py ! ...era angle/20 ma va meglio 18
  {
    head_angle_pub.publish(head_angle);
    //ros::spinOnce();
      r.sleep();
  }

}


/*
  Modifica l'orientazione del robot a nord 0 est 1 sud 2 ovest 3
*/
void changeOrientation(ros::Publisher velocity_pub, ros::Rate r, int c)
{
  int rot_clockwise = orientation-c;
  int rot_contclockwise = rot_clockwise >= 0 ? -4+rot_clockwise : 4+rot_clockwise;

  if (abs(rot_clockwise) < abs(rot_contclockwise))
    rotate(velocity_pub, r, rot_clockwise*pi/2);
  else
    rotate(velocity_pub, r, rot_contclockwise*pi/2);
  
  orientation = c;
}

/*
  implementa tutti i controlli ed i movimenti necessari
  a raggiungere in maniera affidabile una cella scelta tra:
  north 0
  east  1
  south 2
  west  3
*/
bool moveToCell(int c){

  //ROS_INFO("Moving to cell: %d", c);

  ros::Rate r(publication_frequency);
  
  //calibrateGyro(40,r);

  /***** LOGICA DI CAMBIO CELLA ASSUMENDO ASSENZA OSTACOLI ******/

  // ruoto nella direzione della cella da raggiungere
  changeOrientation(velocity_pub, r, c);

  // verifico nuovamente che il sonar non veda ostacoli nella cella in cui voglio andare!
  if ( occupiedCell() )
  {
    ROS_INFO("Occupied Cell! moveToCell ABORTED!");
    return false;
  }

  // avanzo fino a raggiungere la linea bianca
  double distance = goStraight(velocity_pub, r, cell_width*0.7, overTheLine);

  // se ho visto la linea la supero di un po', mi giro a sx fino alla linea, mi giro a dx fino alla linea,
    // mi giro alla metà dell'angolo percorso
  if ( distance < cell_width*0.65 ) // ho visto la linea
  {
  	double overlapping = 0.015;
    
    ROS_INFO("White line REACHED !");
    goStraight(velocity_pub, r, overlapping ); // supero di un po' la linea
    
    double rotation1 = 0.0, rotation2 = 0.0;
    
    rotation1 = rotate(velocity_pub, r, pi*0.6, overTheLine ); // ruoto fin sopra alla linea
    if (rotation1 >= pi*0.6)
    	rotate(velocity_pub, r, -pi*0.6);
    else
    {
    	double rotation2 = rotate(velocity_pub, r, -pi/3);
    	rotation2 += rotate(velocity_pub, r, -pi*0.6, overTheLine ); // ruoto fin sopra alla linea
    	rotate(velocity_pub, r, -rotation2/2.1 ); //ruoto all'indietro di metà dell'angolo
    }
    goStraight(velocity_pub, r, cell_width*0.4 - overlapping ); // avanzo fino al centro della cella
  }
  else
  {
  	goStraight(velocity_pub, r, cell_width*0.3);
    ROS_INFO("White line MISSED !");
  }

  /**************************************************************/
  
  return true;
}


/*
  Ritorna un vettore di 4 interi riferiti alla cella nord, est, sud, ovest.
  Valgono 1 se la cella è occupata, 0 se la cella è libera.
*/
vector<int> getNeighbors() {
  
  int steps = 36;
  int cell_gap = 3;
  vector<int> nesw_neighbors(4);
  ros::Rate r(publication_frequency); 

  for (int i = 0; i<steps; i++) {
    
    if( i%(steps/4) >= cell_gap && i%(steps/4) < steps/4 - cell_gap) {
      rotateHead(head_angle_pub, r, (360.)*i/steps-45);
    
    
      if( occupiedCell() ) {
        cout << range << endl;
        nesw_neighbors[(orientation+((int)floor((360.*i/steps)/90.)))%4] = true;
      }
    }

    cout << ((int)floor((360.*i/steps)/90.)) << endl;
  }
  
  rotateHead(head_angle_pub, r, 0);
  
  return nesw_neighbors;
}

/*
  È il callback utilizzato dal servizio move_to_cell.
*/
bool moveToCellSrv(nxt_lab2::MoveToCell::Request  &req,
         nxt_lab2::MoveToCell::Response &res)
{
 
  if (req.nesw>3 || req.nesw < 0)
  	ROS_ERROR("nesw message must be in range [0,3]. Those numbers correspond to north east south and west");

  ROS_INFO("Moving to cell %d (by robot)",req.nesw);
  
  bool reached = moveToCell(req.nesw);
  
  res.reached = reached; // false se la cella è occupata
  
  return true; // se ritorna false vuol dire che c'è stato un errore a livello di servizio!
}


/*
  È il callback utilizzato dal servizion get_neighbors.
*/
bool getNeighborsSrv(nxt_lab2::Neighbors::Request  &req,
         nxt_lab2::Neighbors::Response &res)
{

  res.neighbors = getNeighbors(); // pare esserci un problema qui

  return true; // se ritorna false vuol dire che c'è stato un errore a livello di servizio!
}


/*
  Calibra il robot ed avvia i servizi necessari alla sua movimentazione.
*/
int main(int argc, char **argv)
{
  ros::init(argc, argv, "robot");
  ROS_INFO("Initializing robot service...");

  ros::NodeHandle n;

  ros::Subscriber gyro_sub, ultrasonic_sensor_sub, color_sensor_sub, joint_states_sub;
  
  gyro_sub = n.subscribe("gyro", 10, callbackGyro); // name, queue size, callback
  ultrasonic_sensor_sub = n.subscribe("ultrasonic_sensor", 1, callbackUltrasonicSensor);
  color_sensor_sub = n.subscribe("intensity_sensor", 100, callbackIntensitySensor);
  joint_states_sub = n.subscribe("joint_states", 100, callbackJointStates);
  
  velocity_pub = n.advertise<geometry_msgs::Twist>("cmd_vel", 1);
  head_angle_pub = n.advertise<std_msgs::Float64>("angle", 100);

  ros::Rate r(publication_frequency);

  calibrateGyro(100,r);
  calibrateIntensitySensor(r);

  ROS_INFO("NxtMov initialized!");
  
  ros::ServiceServer move_to_cell = n.advertiseService("move_to_cell", moveToCellSrv);
  ros::ServiceServer get_neighbors = n.advertiseService("get_neighbors", getNeighborsSrv);

  ros::MultiThreadedSpinner spinner(4);

  spinner.spin();
  
  return 0;
}